package com.re6g3y.dl4j.core;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.Input;
import com.badlogic.gdx.graphics.Color;
import com.badlogic.gdx.math.MathUtils;
import com.badlogic.gdx.math.Vector2;
import com.badlogic.gdx.physics.box2d.*;
import com.mygdx.game.dl4j.MDPEntity;
import com.re6g3y.dl4j.Setting;
import com.re6g3y.dl4j.Utils.BodyUtils;
import org.deeplearning4j.gym.StepReply;
import org.deeplearning4j.nn.multilayer.MultiLayerNetwork;
import org.deeplearning4j.rl4j.space.Encodable;

import java.util.LinkedList;

public class Core {
    public static Core instance;
    public Body car;
//    public ShapeRenderer shapeRenderer=null;
    public GameCore gameCore=null;
//    public Box2DDebugRenderer box2DDebugRenderer=null;
    public int contactFlag=0;
    public MultiLayerNetwork net;

    public Core(World world, GameCore gameCore){
        instance=this;
//        this.net=Learning.getMultiLayerNetwork();
        this.gameCore=gameCore;
//        this.shapeRenderer=gameCore.shapeRenderer;
//        this.world=gameCore.world;
//        this.box2DDebugRenderer=gameCore.box2DDebugRenderer;
        int w=Gdx.graphics.getHeight();
        int h=Gdx.graphics.getHeight();


        //A
        createCircle(new Vector2(550,0),550);
        createCircle(new Vector2(550,0),500);
        Body end=createLine(new Vector2(50,-50),new Vector2(0,-50));
        end.setUserData("end");

        //B
//        int wide=75;
//        int height=300;
//        createLine(new Vector2(0,0),new Vector2(0,height));
//
//        for(int i=0;i<3;i++) {
//            createLine(new Vector2(2*wide*i+wide, 0), new Vector2(2*wide*i+wide, height - wide));
//            createLine(new Vector2(2*wide*(i+1), i==2?0:wide), new Vector2(2*wide*(i+1), height));
//        }
//
//        createLine(new Vector2(0,height),new Vector2(wide*2*3,height));
//        Body end=createLine(new Vector2(wide*2*3,0),new Vector2(wide,0));


        //C
//        float force=30;
//        float size=300;
//        float boxSize=30;
//        createCircle(new Vector2(0,0),size*3);
//        for(int i=0;i<100;i++){
//            Body b=createBox(new Vector2(MathUtils.random(size)-size/2,MathUtils.random(size)-size/2),boxSize,boxSize);
//            b.applyForceToCenter(MathUtils.random(force)-force/2,MathUtils.random(force)-force/2,false);
//        }

        world.setContactListener(new ContactListener() {
            @Override
            public void beginContact(Contact contact) {
                if(contact.getFixtureA().getBody().getUserData().equals("end")||contact.getFixtureB().getBody().getUserData().equals("end")){
                    contactFlag=2;
                }
                if(contact.getFixtureA().getBody().equals(car)||contact.getFixtureB().getBody().equals(car)){
                    contactFlag=1;
                }
            }

            @Override
            public void endContact(Contact contact) {

            }

            @Override
            public void preSolve(Contact contact, Manifold oldManifold) {

            }

            @Override
            public void postSolve(Contact contact, ContactImpulse impulse) {

            }
        });
    }

    public float[] sensorsData=new float[Setting.SensorNumber];
    public LinkedList<Vector2> positionDataList=new LinkedList<Vector2>();
    public final LinkedList<float[]> sensorsDataList=new LinkedList<float[]>();
    private int tick=0;
    private float skip=0;

    public StepReply<Encodable> update(Runnable updatePhyWorld,float value){
        tick++;
        if(skip>0)skip--;
//        key();

        if(skip<=0) {
            sensorsDataList.addFirst(sensorsData);
            positionDataList.addFirst(car.getPosition().cpy());
            sensorsData = new float[Setting.SensorNumber];
            while (sensorsDataList.size() > 100) sensorsDataList.removeLast();
            while (positionDataList.size() > 100) positionDataList.removeLast();

            car.setLinearVelocity(BodyUtils.getForward(car.getAngle(), speed));

//            float val = Learning.getValue(net, sensorsData);
            //左右偏转
//            car.setAngularVelocity(MathUtils.degreesToRadians*speed*val);

        }

        if (value < -1 / 3)
            car.setAngularVelocity(MathUtils.degreesToRadians * speed);
        else if (value > 1 / 3)
            car.setAngularVelocity(-MathUtils.degreesToRadians * speed);
        else car.setAngularVelocity(0);

        for (int i = 0; i < Setting.SkipFrame; i++) updatePhyWorld.run();

        RayCast(gameCore.viewAble);

        int reward=1;
        boolean done=false;

        if(gameCore.viewAble /*||contactFlag!=0*/){
            gameCore.shapeRenderer.begin();
            for(int i=1;i<positionDataList.size();i++){
                if(i/10%2==0)gameCore.shapeRenderer.setColor(Color.YELLOW);
                else gameCore.shapeRenderer.setColor(Color.GREEN);
                gameCore.shapeRenderer.line(positionDataList.get(i), positionDataList.get(i-1));
            }
            gameCore.shapeRenderer.end();
        }
        if(contactFlag!=0){
            if(contactFlag!=2)reward=-1;
            done=true;

            contactFlag=0;

            tick=0;
        }

        double[] cp=new double[sensorsData.length];
        for(int i=0;i<sensorsData.length;i++)cp[i]=sensorsData[i];
        return new StepReply<Encodable>(new MDPEntity(cp),reward,done,null);
    }

    public void reset(){

        contactFlag=0;
        sensorsDataList.clear();
        positionDataList.clear();
        if(car!=null)gameCore.world.destroyBody(car);
        car = createCar();
    }

    public void dispose(){

    }




    public Body createCar(){
        Vector2 position=new Vector2(25,0);
        Body car=createBox(position,25f/3f,50f/3f,0.0f*(MathUtils.random()-0.5f)*MathUtils.PI/6);
        car.setUserData("car");
        return car;
    }

    public void RayCast(final boolean isPrint){
        final int length=100;
        gameCore.shapeRenderer.begin();
        for(int i = 0; i< Setting.SensorNumber; i++) {
            double ang=(Setting.SensorNumber/2-i-(Setting.SensorNumber%2==0?0.5:0))*(240.0/Setting.SensorNumber)*MathUtils.degreesToRadians;
            final int finalI = i;
            sensorsData[finalI]=1;
            gameCore.world.rayCast(new RayCastCallback() {
                int index=0;
                @Override
                public float reportRayFixture(Fixture fixture, Vector2 point, Vector2 normal, float fraction) {
                    if(fixture.getBody().getUserData().equals("end"))return 1;
                    if(isPrint) {
                        gameCore.shapeRenderer.setColor(Color.ROYAL);
                        gameCore.shapeRenderer.line(point, normal.scl(10f).add(point));
                    }
                    sensorsData[finalI]=fraction;
                    return 0;
                }
            }, car.getPosition(), BodyUtils.getForward(car.getAngle()+(float) ang, length).add(car.getPosition()));
            if(isPrint) {
                gameCore.shapeRenderer.setColor(Color.BLUE);
                gameCore.shapeRenderer.line(car.getPosition(), BodyUtils.getForward(car.getAngle() + (float) ang, length).add(car.getPosition()));
                gameCore.shapeRenderer.setColor(Color.PINK);
                gameCore.shapeRenderer.line(car.getPosition(), BodyUtils.getForward(car.getAngle() + (float) ang, length*sensorsData[i]).add(car.getPosition()));
            }
        }
        gameCore.shapeRenderer.end();
    }

    public Body createBall(Vector2 center, float radius){
        Body body;
        BodyDef bd = new BodyDef();
        bd.position.x=center.x;
        bd.position.y=center.y;
        bd.type= BodyDef.BodyType.DynamicBody;
        FixtureDef fixtureDef = new FixtureDef();
        CircleShape cir= new CircleShape();
        body = gameCore.world.createBody(bd);

        body.setBullet(true);
        cir.setRadius(radius);
        fixtureDef.shape = cir;
        fixtureDef.density=0.01f;
        fixtureDef.friction=0.0f;
        fixtureDef.restitution=1.0f;

        body.createFixture(fixtureDef);
        body.setUserData("ball");
        return body;
    }

    public Body createBox(Vector2 center, float w,float h) {
        return createBox(center,w,h,0);
    }
    public Body createBox(Vector2 center, float w,float h,float rotate){
        Body body;
        BodyDef bd = new BodyDef();
        bd.position.x=center.x;
        bd.position.y=center.y;
        bd.angle=rotate;
        bd.type= BodyDef.BodyType.DynamicBody;
        FixtureDef fixtureDef = new FixtureDef();
        PolygonShape shape= new PolygonShape();
        shape.setAsBox(w,h);
        body = gameCore.world.createBody(bd);

        body.setBullet(true);
        fixtureDef.shape = shape;
        fixtureDef.density=0.01f;
        fixtureDef.friction=0.0f;
        fixtureDef.restitution=1.0f;

        body.createFixture(fixtureDef);
        body.setUserData("box");
        return body;
    }

    public Body createLine(Vector2 start,Vector2 end){
        Body body;
        BodyDef bd = new BodyDef();
        FixtureDef boxShapeDef = new FixtureDef();
        EdgeShape edge = new EdgeShape();
        body = gameCore.world.createBody(bd);

        edge.set(start,end);
        boxShapeDef.shape = edge;

        body.createFixture(boxShapeDef);
        body.setUserData("rect");
        return body;
    }

    public void createCircle(Vector2 center,float radius){
        float k=1f/30;
        for(float i=0;i<1;i+=k){
            float start=i*MathUtils.PI2,end=(i+k)*MathUtils.PI2;
            createLine(BodyUtils.getForward(start,radius).add(center),BodyUtils.getForward(end,radius).add(center)).setType(BodyDef.BodyType.StaticBody);
        }
    }


    float speed=30;
    public void key(){
        if(Gdx.input.isKeyPressed(Input.Keys.W)){
            car.setLinearVelocity(BodyUtils.getForward(car.getAngle(),speed));
        }
        else if(Gdx.input.isKeyPressed(Input.Keys.S)){
            car.setLinearVelocity(new Vector2(MathUtils.sin(car.getAngle())*speed,-MathUtils.cos(car.getAngle())*speed));
        }else {
            car.setLinearVelocity(Vector2.Zero);
        }

        if(Gdx.input.isKeyPressed(Input.Keys.A)){
            car.setAngularVelocity(MathUtils.degreesToRadians*speed);
        }
        else if(Gdx.input.isKeyPressed(Input.Keys.D)){
            car.setAngularVelocity(-MathUtils.degreesToRadians*speed);
        }
        else car.setAngularVelocity(0);
    }
}
